﻿/*
 * CL2C-RS系列驱控一体驱动器，只能控制单个电机，使用PR模式（位置寄存器）控制电机运动
 * 本模块根据自身需求，采用PR0寄存器控制电机运动,PR9.00,寄存器基地址为0x6200
*/

#include "CL2CRSDriveControl.h"
#include <QDebug>
#include <QElapsedTimer>
#include <QThread>
#include <QCoreApplication>

#include "CommunicationManager.h"
#include "bitutils.h"


CL2CRSDriveControl::CL2CRSDriveControl(QObject *parent)
    :BaseMotorController(parent)
{
    _stateItems={
            StateItem("故障"),
            StateItem("使能"),
            StateItem("运行"),
            StateItem("无效"),
            StateItem("指令完成"),
            StateItem("路径完成"),
            StateItem("回零完成"),
            StateItem("当前报警","01:过流 02:过压 40:电流采样回路故障 80:锁轴 200:EEPROM 100:参数自整定故障"),
            StateItem("当前位置"),
            StateItem("当前速度"),
            StateItem("限位报警","100:限位故障 102:超程 20*:路径*限位故障"),
            StateItem("DO0","25:正向常开 26:负向常开 a5:正向常闭 a6:负向常闭"),
            StateItem("DO1","25:正向常开 26:负向常开 a5:正向常闭 a6:负向常闭"),
            StateItem("峰值电流","503:0-30 507:0-70,单位0.1A"),
            StateItem("电机运行方向","0:正方向 1:负方向")
    };
    _typeAddressMap=
    {
        {VEL_TOPSPEED,0x6203},
        {VEL_ACCSPEED,0x6204},
        {VEL_INITSPEED,0x6205}
    };
}

CL2CRSDriveControl::~CL2CRSDriveControl()
{    
}

void CL2CRSDriveControl::SetCurrentAxisType(BaseMotorController::AxisType axisType)
{
    _currentAxisType=axisType;
    if(_communications.count()==1)
    {
        _currentAxisType=AxisType::AXIS_X;
    }
}

void CL2CRSDriveControl::SetMotorEnable(bool enable)
{
    uint16_t val = (uint16_t)(enable ? 1 : 0);
    WriteRegister(0x000f,val);
}

int CL2CRSDriveControl::SetVelocity(VEL_TYPE_ENUM eVelType, double dVelocity)
{
    double dtempVel = dVelocity;
    QMap<VEL_TYPE_ENUM,double> maxMap=
    {
        {VEL_TOPSPEED,6},
        {VEL_ACCSPEED,10},
        {VEL_INITSPEED,10}
    };
    dtempVel=(dtempVel>maxMap[eVelType])?maxMap[eVelType]:dtempVel;
    dtempVel *= 100;
    WriteRegister(_typeAddressMap[eVelType],static_cast<uint16_t>(dtempVel));
    return 0;
}


int CL2CRSDriveControl::RelativeMove(double dRelativeMove)
{
    SetMotorEnable(true);
    WriteRegister(0x6200, 0x41);
    int tempPos = (int)(dRelativeMove * 10000);
    //写入位置
    WriteRegisters(0x6201,ConvertToUint16List(tempPos));
    //启动
    WriteRegister(0x6002, 0x0010);

    return 0;
}

int CL2CRSDriveControl::AbsoluteMove(double dMove)
{
    SetMotorEnable(true);
    WriteRegister(0x6200,0x01);
    int tempPos = (int)(dMove * 10000);
    //写入位置
    WriteRegisters(0x6201,ConvertToUint16List(tempPos));
    //启动
    WriteRegister(0x6002, 0x0010);
    return 0;
}

void CL2CRSDriveControl::SetCommunication(QList<BaseCommunication *> communication)
{
    BaseDevice::SetCommunication(communication);
    //单轴
    if(communication.count()>0)
    {
        for(int i=0;i<communication.count();i++)
        {
            BaseModbus *modbus=dynamic_cast<BaseModbus*>(communication.at(i));
            if(nullptr==modbus)
            {
                QString err="CL2CRSDriveControl 配置:"+communication.at(0)->Name()+"通讯失败!";
                throw QString(err);
            }
            _myModbus.append(dynamic_cast<BaseModbus*>(communication.at(i)));
        }
    }
    SetVelocity(VEL_TOPSPEED,10);
}

bool CL2CRSDriveControl::Stop()
{
//    return WriteRegister(0x6002,0x0040);
    return true;
}

bool CL2CRSDriveControl::SaveParam()
{
    return WriteRegister(0x1801, 0x2211);
}

bool CL2CRSDriveControl::Reset()
{
    return WriteRegister(0x1801, 0x2233);
}

void CL2CRSDriveControl::GetConnectStateCore(DeviceStateItems &items)
{
    double pos;
    GetPosition(pos);
    items[0].Data=QString::number(pos);
}

void CL2CRSDriveControl::SetMaxElectricity(int electricity)
{
    bool ret=WriteRegister(0x0191,electricity);
}

void CL2CRSDriveControl::SetAxisDriection(BaseMotorController::AxisDirectionType axisType)
{
    int val=axisType==AxisDirectionType::AxisForward?0:1;
    WriteRegister(0x0007,val);
}

int CL2CRSDriveControl::SetElectricityPercentage(int percentage)
{
    bool ret=WriteRegister(0x0193,percentage);
    return ret?0:-1;
}

void CL2CRSDriveControl::SetLimit(QList<MotorIOItem> items)
{
    int startAddr = 0x0145;
    for (int i = 0; i < items.count(); i++)
    {
        int val = items[i].PloartyType == PolartyType::Open ? items[i].FunctionType : items[i].FunctionType + 0x80;
        WriteRegister(startAddr + i * 2, (uint16_t)val);
    }
}

void CL2CRSDriveControl::ReturnToZero(BaseMotorController::DirectionType direction, LimitType limit)
{
    QVector<bool> bArr(16,false);
    bArr[0] = direction == DirectionType::Forward;
    bArr[2] = limit == LimitType::Zero;
    WriteRegister(0x600A,BitUtils::GetNum(bArr));
    WriteRegister(0x600f, 0x0064);
    WriteRegister(0x6010, 0x001e);
    WriteRegister(0x6002, 0x0020);
}

QList<StateItem> CL2CRSDriveControl::GetStateItems()
{
    uint16_t state;
    ReadOutRegister(0x1003, state);
    for (int i = 0; i < 7; i++)
    {
        bool stateb = BitUtils::GetBit(state, i);
        _stateItems[i].State = stateb ? "1" : "0";
    }

    uint16_t error;
    ReadOutRegister(0x2203,error);
    _stateItems[7].State=QString::number(error,16);

    double pos;
    GetPosition(pos);
    _stateItems[8].State=QString::number(pos);

    uint16_t vel;
    ReadOutRegister(0x6203,vel);
    double dVel=vel/100.0;
    _stateItems[9].State=QString::number(dVel);

    uint16_t alarm;
    ReadOutRegister(0x601d,alarm);
    _stateItems[10].State=QString::number(alarm,16);

    QList<uint16_t> limits;
    ReadOutRegisters(0x0145,3,limits);
    _stateItems[11].State=QString::number(limits[0],16);
    _stateItems[12].State=QString::number(limits[2],16);

    uint16_t maxElectricity;
    ReadOutRegister(0x0191,maxElectricity);
    _stateItems[13].State=QString::number(maxElectricity,10);

    uint16_t axisDirection;
    ReadOutRegister(0x0007,axisDirection);
    _stateItems[14].State=QString::number(axisDirection,10);
    return _stateItems;
}

QList<uint16_t> CL2CRSDriveControl::ConvertToUint16List(int data)
{
    return BitUtils::ConvertToUint16List(data);
}

int CL2CRSDriveControl::SetCurrentPositionToZero()
{
    WriteRegister(0x6002,0x0021);
    return 0;
}

int CL2CRSDriveControl::GetPosition(double &dPosition)
{
    int iErrorCode = 0;
    QList<uint16_t> val;
    bool ret=ReadOutRegisters(0x602C,2,val);
    if(ret) {
        dPosition = BitUtils::ConvertToInt(val);
        dPosition /=10000.0;
    } else {
        iErrorCode = -1;
    }
    return iErrorCode;
}

bool CL2CRSDriveControl::WriteRegister(int addr,uint16_t value)
{
    auto _modbusX=GetCurrentModbus();
    QList<uint16_t> val{value};
    if(_modbusX->IsConnected())
    {
        return _modbusX->WriteRegisters(addr,1,val);
    }
    else
    {
        return false;
    }
}

bool CL2CRSDriveControl::WriteRegisters(int addr,QList<uint16_t> value)
{
    auto _modbusX=GetCurrentModbus();
    if(_modbusX->IsConnected())
    {
        return _modbusX->WriteRegisters(addr,value.count(),value);
    }
    else
    {
        return false;
    }
}

bool CL2CRSDriveControl::ReadOutRegisters(int addr,int count,QList<uint16_t> &value)
{
    auto _modbusX=GetCurrentModbus();
    if(_modbusX->IsConnected())
    {
        return _modbusX->ReadOutRegisters(addr,count,value);
    }
    else
    {
        return false;
    }
}

bool CL2CRSDriveControl::ReadOutRegister(int addr, uint16_t &value)
{
    auto _modbusX=GetCurrentModbus();
    if(_modbusX->IsConnected())
    {
        return _modbusX->ReadOutRegister(addr,value);
    }
    else
    {
        return false;
    }
}

BaseModbus *CL2CRSDriveControl::GetCurrentModbus()
{
    if(_currentAxisType==AxisType::AXIS_X)
    {
        return _myModbus.at(0);
    }
    else if(_currentAxisType==AxisType::AXIS_Y)
    {
        return _myModbus.at(1);
    }
    else if(_currentAxisType==AxisType::AXIS_Z)
    {
        return _myModbus.at(2);
    }
    throw QString("无对应通讯实体!");
}


